#include <QDebug>
#include "collision.h"
#include "math.h"

Collision::Collision()
{
}

double Collision::pointToLineDistance(QLineF line, QPointF point)
{
    QPointF pa = line.p1();
    QPointF pb = line.p2();

    double A = pb.ry() - pa.ry();
    double B = pa.rx() - pb.rx();
    double C = (pb.rx()*pa.ry()) - (pb.ry()*pa.rx());

    return fabs(A*point.rx() + B*point.ry() + C)/sqrt(A*A + B*B);
}

QList<QGraphicsPolygonItem*> Collision::getCollidingPolygons(QGraphicsItem* p)
{
    QList<QGraphicsItem*> items = p->collidingItems();


    QList<QGraphicsPolygonItem*> polys;
    polys.clear();
    if (items.count() > 0)
    {
        for (int i = 0; i < items.count(); i++)
        {
            QGraphicsPolygonItem* poly = qgraphicsitem_cast<QGraphicsPolygonItem*>(items.at(i));

            if (poly != 0)
            {

                polys.append(poly);
            }
        }
    }
    return polys;
}

QList<Player*> Collision::getCollidingPlayers(QGraphicsItem* p)
{
    QList<QGraphicsItem*> items = p->collidingItems();


    QList<Player*> players;
    players.clear();
    if (items.count() > 0)
    {
        for (int i = 0; i < items.count(); i++)
        {
            Player* player = qgraphicsitem_cast<Player*>(items.at(i));

            if (player != 0)
            {

                players.append(player);
            }
        }
    }
    return players;
}

int Collision::getPolygonCollisionCount(QGraphicsItem* p)
{
    QList<QGraphicsItem*> items = p->collidingItems();


    int count = 0;
    if (items.count() > 0)
    {
        for (int i = 0; i < items.count(); i++)
        {
            QGraphicsPolygonItem* poly = qgraphicsitem_cast<QGraphicsPolygonItem*>(items.at(i));

            if (poly != 0)
            {

                count++;
            }
        }
    }
    return count;
}

double Collision::getClosestIntersectionPointOnPolygons(QLineF l, QList<QGraphicsPolygonItem*> polys, QPointF *intPoint)
{

    double lineLen= -1;
    double minLineLen = -1;
    int closestLine = -1;
    for (int i = 0; i < polys.count(); i++)
    {
        QGraphicsPolygonItem *poly = polys.at(i);
        int pointCount = poly->polygon().size();
        QLineF line[pointCount];
        for (int j = 0; j < pointCount; j++) {
            line[j] = QLineF(poly->polygon().at(j), poly->polygon().at((j+1)%pointCount));
        }


        for (int j = 0; j < pointCount; j++)
        {
            QPointF*intPointRob = new QPointF();
            if (l.intersect(line[j],intPointRob) == QLineF::BoundedIntersection) {
                lineLen = QLineF(l.p1(),*intPointRob).length();

                if (lineLen < minLineLen || (closestLine < 0))
                {
                    minLineLen = lineLen;
                    closestLine = j;
                    intPoint->setX(intPointRob->x());
                    intPoint->setY(intPointRob->y());
                }
            }



        }
    }
    return minLineLen;
}

double Collision::distanceFromSegment(QLineF line, QPointF point, int &voronoiPoint, double & angle)
{
    double cx = point.rx();
    double cy = point.ry();
    double ax = line.p1().rx();
    double ay = line.p1().ry();
    double bx = line.p2().rx();
    double by = line.p2().ry();
    double r_numerator = (cx-ax)*(bx-ax) + (cy-ay)*(by-ay);
    double r_denomenator = (bx-ax)*(bx-ax) + (by-ay)*(by-ay);
    double r = r_numerator / r_denomenator;

    double px = ax + r*(bx-ax);
    double py = ay + r*(by-ay);

    double s =  ((ay-cy)*(bx-ax)-(ax-cx)*(by-ay) ) / r_denomenator;

    double distanceLine = fabs(s)*sqrt(r_denomenator);

    double xx = px;
    double yy = py;

    if ( (r >= 0) && (r <= 1) )
    {
        voronoiPoint = 0;
        angle = QLineF(xx,yy,cx,cy).angle();
        return distanceLine;
    }
    else
    {

        double dist1 = (cx-ax)*(cx-ax) + (cy-ay)*(cy-ay);
        double dist2 = (cx-bx)*(cx-bx) + (cy-by)*(cy-by);
        if (dist1 < dist2)
        {
            xx = ax;
            yy = ay;
            angle = QLineF(xx,yy,cx,cy).angle();
            voronoiPoint = 1;
            return sqrt(dist1);
        }
        else
        {
            xx = bx;
            yy = by;
            angle = QLineF(xx,yy,cx,cy).angle();
            voronoiPoint = 2;
            return  sqrt(dist2);
        }
    }

    return -1;
}

void Collision::checkCollision(Player * p)
{


    QList<QGraphicsItem*> items = p->collidingItems();
    QLineF vector(0,0,0,0);
    if (items.count() > 0)
    {
        for (int i = 0; i < items.count(); i++)
        {
            QGraphicsPolygonItem* poly = qgraphicsitem_cast<QGraphicsPolygonItem*>(items.at(i));

            if (poly != 0)
            {

                QPointF playerPos(p->getPos());
                if (!poly->polygon().containsPoint(playerPos,Qt::OddEvenFill)) {
                    int pointCount = poly->polygon().count();
                    QLineF line[pointCount];
                    for (int j = 0; j < pointCount; j++) {
                        line[j] = QLineF(poly->polygon().at(j), poly->polygon().at((j+1)%pointCount));
                    }

                    double minLineLen = -1;
                    int closestLine = -1;
                    double angle;
                    int vr;

                    for (int j = 0; j < pointCount; j++)
                    {
                        double angleRob;
                        int vrRob;
                        double lineLen =distanceFromSegment(line[j], playerPos, vrRob, angleRob);

                        if (lineLen < minLineLen || (closestLine < 0))
                        {
                            minLineLen = lineLen;
                            closestLine = j;
                            vr = vrRob;
                            angle = angleRob;
                        }

                    }
                    //if (minLineLen >= 10) return false;


                    if (vr == 0) {

                        vector.setP1(QPointF(0,0));
                        vector.setP2(QPointF(p->getRadius()-minLineLen, 0));
                        vector.setAngle(angle);

                        p->movePlayerBy(vector);

                    }
                    else if (vr == 1) {

                        vector.setP1(QPointF(0,0));
                        vector.setP2(QPointF(p->getRadius()-QLineF(playerPos, line[closestLine].p1()).length(), 0));
                        vector.setAngle(angle);

                        p->movePlayerBy(vector);
                    }
                    else {

                        vector.setP1(QPointF(0,0));
                        vector.setP2(QPointF(p->getRadius()-QLineF(playerPos, line[closestLine].p2()).length(), 0));
                        vector.setAngle(angle);

                        p->movePlayerBy(vector);
                    }


                }

            }
        }
    }
}
